home *** CD-ROM | disk | FTP | other *** search
/ C/C++ Users Group Library 1996 July / C-C++ Users Group Library July 1996.iso / vol_300 / 341_01 / twobdy.c < prev    next >
Encoding:
C/C++ Source or Header  |  1991-02-26  |  15.5 KB  |  471 lines
  1. /*
  2. HEADER:       twobdy.c        
  3. TITLE:        Two-body, Keplerian Orbit Propagator which uses
  4.               universal variables formulation.         
  5. VERSION:      1.0    
  6. DESCRIPTION:  This routine propagates Cartesian position and velocity
  7.               for a specified time interval.  It will optionally
  8.               output partial derivative data.
  9.               Required inputs are the initial Cartesian position and velocity,
  10.               the time interval to propagate, mu for the central body,
  11.               an initial guess for the solution to the generalized Kepler
  12.               equation, and a switch to indicate whether partial derivative
  13.               data is being requested.  
  14. KEYWORDS:     Astrodynamics, orbital mechanics, orbit propagation,
  15.               universal variables, Goodyear method, Cartesian coordinates    
  16. SYSTEM:       MS-DOS, PC-DOS (Coded with Ver. 3.3, but should be version
  17.               independent)         
  18. FILENAME:     twobdy.c    
  19. UNITS:        I believe that the code is not dependent on any particular
  20.               set of units.  See orbcons.h for the value of earth mu
  21.               I use.
  22. SEE-ALSO:     k2ce.c
  23. WARNINGS:     This code was coded for educational purposes.  No attempt
  24.               has been made to provide for optimal numerical processing.
  25.               For a detailed reference to the algorithm, see the second
  26.               reference.
  27. AUTHORS:      Rodney Long
  28.               19003 Swan Drive
  29.               Gaithersburg, MD 20879     
  30. COMPILERS:    Microsoft C 5.1    
  31. REFERENCE:    Bate, Mueller, White: Fundamentals of Astrodynamics
  32.               Goodyear, W.H.,"A General Method for the
  33.               Computation of Cartesian Coordinates and
  34.               Partial Derivatives of the Two-Body Problem",
  35.               NASA Contractor Report NASA CR 522.
  36.  
  37. */
  38. #include <io.h>
  39. #include <fcntl.h>
  40. #include <sys\types.h>
  41. #include <sys\stat.h>
  42. #include <stdio.h>
  43. #include <math.h>
  44. #include <float.h>
  45. #include "orbcons.h"
  46.  
  47.  
  48. struct input {
  49.   double s0[6];         // s0[6]: Pos/vel at time t0
  50.   double tau;           // Time interval from t0 to t
  51.   double mu;            // Mu from diff. eq. of motion
  52.   double psi;              // Initial approx for sol. of
  53.                         //  Kepler's equation.
  54.   int    auxind;        // 0=output pos/vel and final psi.
  55.                         //  1=output auxiliary data.
  56. } ;
  57.  
  58. struct output {
  59.   double psi;            // Final sol. to Kepler's equation.
  60.   double s[6];             // s[6]: Pos/vel at time t.
  61. } ;
  62.  
  63. struct aux_output {
  64.   double p[6][6];           // p[6][6]: Partial derivs. of
  65.                         //   s with respect to s0.
  66.   double pi[6][6];       // pi[6][6]: Inverse of p. 
  67.   double pmu[6];            // pmu[6]: Partial derivs. of
  68.                         //   s with respect to mu.
  69.   double p0mu[6];            // p0mu[6]: Partial derivs. of
  70.                         //   s0 with respect to mu.
  71.   double acc[3];            // acc[3]: Acceleration at time t.
  72.   double acc0[3];            // acc0[3]: Accel. at time t0.
  73.   double r;                // Orbit radius at time t.
  74.   double r0;               // Orbit radius at time t0.
  75. }
  76.  
  77.  
  78.  
  79. main()
  80. {
  81.   int i,n;
  82.   char filename[31];
  83.   FILE *fp;
  84.  
  85.  
  86.   struct input in;
  87.   struct output out;
  88.   struct aux_output aux;
  89.   
  90.   printf("Enter input file name: \n");
  91.   gets(filename);
  92.  
  93.  
  94.   fp = fopen(filename,"r");
  95.   printf("filename = %s\n",filename);
  96.   if ( fp == NULL) {
  97.     perror("Open error \n");
  98.     exit(0);
  99.   }
  100.   fscanf(fp,"%lf %lf %lf",&(in.s0[0]),&(in.s0[1]),&(in.s0[2]));
  101.   fscanf(fp,"%lf %lf %lf",&(in.s0[3]),&(in.s0[4]),&(in.s0[5]));
  102.   fscanf(fp,"%lf",&(in.tau)); 
  103.   fscanf(fp,"%lf",&(in.mu)); 
  104.   fscanf(fp,"%lf",&(in.psi)); 
  105.   fscanf(fp,"%d",&(in.auxind));
  106.  
  107.   printf("%25.16lf %25.16lf %25.16lf\n",in.s0[0],in.s0[1],in.s0[2]);
  108.   printf("%25.16lf %25.16lf %25.16lf\n",in.s0[3],in.s0[4],in.s0[5]);
  109.   printf("%25.16lf\n",in.tau); 
  110.   printf("%25.16lf\n",in.mu); 
  111.   printf("%25.16lf\n",in.psi); 
  112.   printf("%d\n",in.auxind);
  113.  
  114.  
  115.   // For the sample orbit print out the pos and velocity
  116.   //   at each quarter-orbit point.
  117.   // The orbit period has been computed from
  118.   //   period = ( TWOPI/sqrt(mu) ) * a**1.5.
  119.   // The number given below is one quarter of the orbit
  120.   //   period for the sample case, in seconds. 
  121.  
  122.   // NOTE THAT FOR THIS SPECIAL TEST, in.tau from the
  123.   //  input file is overwritten.  In the normal case, you
  124.   //  would remove the "for" loop below and just leave the
  125.   //  call to twobdy() and the print of the outputs.  Note
  126.   //  that if you want the partial derivative output, you must
  127.   //  add print statements to output them.
  128.  
  129.   for (i=0; i<4; i++) {
  130.     in.tau = (i+1)*1576.780105;    
  131.     twobdy(&in,&out,&aux);
  132.  
  133.   printf("Output pos/vel: \n");
  134.   printf("%25.16lf %25.16lf %25.16lf\n",out.s[0],out.s[1],out.s[2]); 
  135.   printf("%25.16lf %25.16lf %25.16lf\n",out.s[3],out.s[4],out.s[5]);
  136.   }
  137. }
  138.  
  139. twobdy(struct input *in, struct output *out, struct aux_output *aux)
  140.  
  141.  
  142. //  twobdy:  A function to compute the position and
  143. //           velocity of an orbiting body under
  144. //           two-body motion, given the position and
  145. //           velocity at time t0, the elapsed time tau
  146. //           at which to compute the solution, the value
  147. //           for mu for the two bodies, and an initial
  148. //           approximation psi of the solution to
  149. //           the generalized Kepler's equation.
  150. //
  151. //  References:
  152. //    (1)  Goodyear, W.H.,"A General Method for the
  153. //         Computation of Cartesian Coordinates and
  154. //         Partial Derivatives of the Two-Body Problem",
  155. //         NASA Contractor Report NASA CR 522.
  156. //    (2)  Bate, Mueller, White, Fundamentals of
  157. //         Astrodynamics. 
  158.  
  159. {
  160.  
  161. #define SQ(x) (x*x)
  162. #define LARGENUMBER 1.e+300
  163.  
  164.  
  165. // Additional variables
  166.    int    its;                      // Iteration counter
  167.    double a, ap;            // Argument in series
  168.                                     //   expansion.
  169.    double alpha, sig0, u;           // Temporary quantities
  170.    double c0, c1, c2, c3, c4, c5x3, // Series coefficents
  171.           s1, s2, s3;             
  172.    double psin, psip;               // Acceptance bounds for
  173.                                     //   sol. psi to Kep. eq.    
  174.    double dtau,dtaun, dtaup;        // Residuals in Newton
  175.                                     //  method for solving
  176.                     //  Kep. eq. 
  177.    double fm1, g;                   // f,g &
  178.    double fd, gdm1;                //   fdot, gdot express.
  179.  
  180.    double acc[3] , acc0[3] ,
  181.           mu     ,                  // Local copies of i/o
  182.           p[6][6], pi[6][6],
  183.              psi    ,                  //   variables
  184.           r      , r0      ,
  185.           s[6]   , s0[6]   ,
  186.           tau;          
  187.  
  188.    int loopexit;                    // Flag to exit main
  189.                                     //   program loop.
  190.  
  191.    int i,j,m;
  192.  
  193. // Copy the input pos, vel, tau, psi into
  194. //  local variables.
  195.    for (i=0; i<6 ; i++)
  196.      s0[i] = in->s0[i];
  197.     mu  = in->mu;
  198.    tau = in->tau;
  199.    psi = in->psi;
  200.  
  201.   
  202. //       Compute initial orbit radius
  203.     r0    = sqrt(SQ(s0[0]) + SQ(s0[1]) + SQ(s0[2]));
  204. //       Compute other intermediate quantities.
  205.     sig0  = s0[0]*s0[3] + s0[1]*s0[4] + s0[2]*s0[5];
  206.     alpha = SQ(s0[3]) + SQ(s0[4]) + SQ(s0[5]) - 2*mu/r0;
  207. //       Initialize series mod count m to zero
  208.     m = 0;
  209.  
  210.  
  211. //---Establish initial psi and initial acceptance bounds
  212. //---for psi.                                             
  213.     if (tau == 0) {              // If input tau = 0, set
  214.       psi = 0;                      //  output psi to 0; else
  215.     } else {                      //  initialize acceptance
  216.       if (tau < 0) {             //  bounds for psi, and
  217.           psin  = -LARGENUMBER;      //  initialize Newton
  218.              psip  = 0;    //  method iteration 
  219.           dtaun = psin;              //  residuals.
  220.           dtaup = -tau;
  221.         } else {                  //tau > 0
  222.           psin = 0;
  223.           psip = LARGENUMBER;
  224.           dtaun = -tau;
  225.           dtaup = psip;
  226.         }
  227.  
  228.         // If the input psi lies between bounds
  229.         //  psin and psip, use it as a first approx.
  230.         //  to a solution of Kepler's equation.
  231.         // If not, we adjust the input psi before using
  232.         //  it to solve Kepler.
  233.  
  234.         if (!( (psi > psin) && (psi < psip)) ) {
  235.           // Try Newton's method for initial psi set equal to zero  
  236.           psi = tau/r0;
  237.           // Set psi = tau if Newton's method fails
  238.           if (psi <= psin || psi >= psip)
  239.             psi = tau;
  240.         }
  241.      }
  242. //---
  243.     loopexit = 0;
  244.     its      = 0;
  245. //-----  BEGINNING OF LOOP FOR SOLVING GENERALIZED KEP. EQ.
  246.     do {
  247.       its++;
  248.       a = alpha * psi * psi;
  249.       if (fabs(a) > 1) {
  250.         ap = a;              // Save original value of a
  251.         // Iterate until abs(a) <= 1.
  252.         while (fabs(a) > 1) {
  253.           m++;              // Keep track of number of
  254.                             //  times reduce a.
  255.           a *= 0.25;
  256.         }
  257.       }
  258.      // Now compute "C series" terms:
  259.      //   Note that they are functions of psi.
  260.      c5x3 = (1+(1+(1+(1+(1+(1+(1+a/342)*a/272)*a/210)*a/156)
  261.             *a/110)*a/72)*a/42)/40;
  262.      c4   = (1+(1+(1+(1+(1+(1+(1+a/306)*a/240)*a/182)*a/132)
  263.             *a/90)*a/56)*a/30)/24;
  264.      c3 = (.5 + a*c5x3)/3;
  265.      c2 = (.5 + a*c4);
  266.      c1 = 1 + a*c3;
  267.      c0 = 1 + a*c2;
  268.  
  269.      //   If we reduced "a" above, we have to adjust the
  270.      //     C terms just computed.
  271.      if (m>0) {
  272.        while (m>0) {
  273.          c1 = c1*c0;
  274.          c0 = 2*c0*c0 - 1;
  275.          m--;
  276.         }
  277.  
  278.        c2 = (c0 -  1)/ap;
  279.        c3 = (c1 -  1)/ap;
  280.        c4 = (c2 - .5)/ap;
  281.        c5x3 = (3 * c3-.5)/ap;
  282.      }
  283.  
  284.      // Now use the C terms to compute the "S series" terms.
  285.      s1 = c1 * psi;
  286.      s2 = c2 * psi * psi;
  287.      s3 = c3 * psi * psi * psi;
  288.  
  289.      g    = r0*s1 + sig0*s2;   // Compute g; used later to
  290.                                //   get position.
  291.  
  292.      // Compute dtau, the function to be zeroed by the
  293.      //   Newton method.
  294.      dtau = (g + mu*s3) - tau;
  295.      // r is the derivative of dtau with respect to psi.
  296.      r    = fabs(r0*c0 + (sig0*s1 + mu*s2));
  297.  
  298.      // Check for convergence of iteration and
  299.      //  reset bounds for psi.
  300.       if (dtau == 0) {
  301.         loopexit = 1;
  302.         continue;            // Get out of loop if dtau==0. 
  303.       } 
  304.       else if (dtau < 0) {
  305.         psin  = psi;
  306.         dtaun = dtau;
  307.       }
  308.       else {                  // dtau > 0
  309.         psip = psi;
  310.         dtaup = dtau;
  311.       }
  312.       
  313.       // This is the Newton step:
  314.       //   x(n+1) = x(n) - y(n)/(dy/dx).
  315.       psi = psi - dtau/r;
  316.  
  317.  
  318.       // Accept psi for further iterations if it is
  319.       // between bounds psin and psip.
  320.       if ( (psi > psin) && (psi < psip) ) continue;
  321.  
  322. //--  -- -- Begin modifications to Newton method.
  323.       // Try incrementing bound with dtau nearest zero by
  324.       // the ratio 4*dtau/tau.
  325.       if ( fabs(dtaun) < fabs(dtaup) )
  326.         psi = psin * (1 - (4*dtaun)/tau);
  327.       if ( fabs(dtaup) < fabs(dtaun) )
  328.         psi = psip * (1 - (4*dtaup)/tau);
  329.       if ( (psi > psin) && (psi < psip) ) continue;
  330.  
  331.       // Try doubling bound closest to zero.
  332.       if (tau > 0)
  333.         psi = psin + psin; 
  334.         if (tau < 0)
  335.         psi = psip + psip;
  336.       if ( (psi > psin) && (psi < psip) ) continue;
  337.  
  338.       // Try interpolation between bounds.
  339.       psi = psin + (psip - psin) * (-dtaun/(dtaup - dtaun));
  340.       if ( (psi > psin) && (psi < psip) ) continue;
  341.  
  342.       // Try halving between bounds.
  343.       psi = psin + (psip - psin) * .5;
  344.       if ( (psi > psin) && (psi < psip) ) continue;
  345. //-- -- --
  346.       // If we still are not between bounds, we've improved
  347.       //  the estimate of psi as much as possible.
  348.       loopexit = 1;
  349.       
  350.     } while ( loopexit == 0 && its <= 10);
  351. //-----  END OF LOOP FOR SOLVING GENERALIZED KEPLER EQ.
  352.  
  353.     // Compute remaining three of four functions fm1, g, fd,
  354.     //   gdm1
  355.     fm1  = -mu * s2 / r0;
  356.     fd   = -mu * s1 / ( r0 * r);
  357.     gdm1 = -mu * s2 / r;
  358.  
  359.     // Compute output solution for time t = t0 + tau
  360.     for (i=0;i<3;i++) {
  361.       // Position solution    at time t
  362.       out->s[i]  = s[i] = s0[i] + (fm1 * s0[i] + g * s0[i+3]);
  363.       // Velocity solution at time t
  364.       out->s[i+3] = s[i+3] = (fd * s0[i] + gdm1 * s0[i+3])
  365.                                               + s0[i+3];
  366.       // Generalized eccentric anomaly for time t
  367.       in->psi = psi;
  368.     }
  369.     // Output additional outputs if requested by user.
  370.     if (in->auxind) {
  371.       // Acceleration solution at time t
  372.       aux->acc[i]  = acc[i]  = -mu * s[i]  / (r*r*r);
  373.       // Acceleration solution at original time t0
  374.       aux->acc0[i] = acc0[i] = -mu * s0[i] / (r*r*r);
  375.         // Orbit radii at t and at t0
  376.       aux->r0 = r0;
  377.       aux->r  = r;
  378.  
  379.       // Partial derivative computations
  380.  
  381.       // Compute coefficients for state partials
  382.       u   = s2*tau + mu*(c4 - c5x3) * psi*psi*psi*psi*psi;
  383.       p[0][0] = -(fd*s1 + fm1/r0)/r0;
  384.       p[0][1] = -fd*s2;
  385.       p[1][0] =  fm1*s1/r0;
  386.       p[1][1] =  fm1*s2;
  387.       p[0][2] =  p[0][1];
  388.       p[0][3] = -gdm1*s2;
  389.       p[1][2] =  p[1][1];
  390.       p[1][3] =  g*s2;
  391.       p[2][0] = -fd*(c0/(r0*r) + 1/(r*r) + 1/(r0*r0));
  392.       p[2][1] = -(fd*s1 + gdm1/r)/r;
  393.       p[3][0] = -p[0][0];
  394.       p[3][1] = -p[0][1];
  395.       p[2][2] =  p[2][1];
  396.       p[2][3] = -gdm1 + s1/r;
  397.       p[3][2] = -p[0][1];
  398.       p[3][3] = -p[0][3];
  399.  
  400.       // Compute coefficients for mu partials
  401.       p[0][4] = -s1 / (r0*r);
  402.       p[1][4] =  s2 / r0;
  403.       p[2][4] =  u  / r0 - s3;
  404.       p[0][5] = -p[0][4];
  405.       p[1][5] =  s2 / r;
  406.       p[2][5] = -u  / r + s3;
  407.  
  408.       // Compute mu partials
  409.        for (i=0;i<3;i++) {
  410.         aux->pmu[i]    = -s[i]  * p[1][4]
  411.                               + s[i+2]  *  p[2][4];
  412.         aux->pmu[i+2]  =  s[i]  * p[0][4]
  413.                               + s[i+2]  *  p[1][4]
  414.                               + acc[i]  *  p[2][4];
  415.         aux->p0mu[i]   = -s0[i] * p[1][5]
  416.                               + s0[i+2] *  p[2][5];
  417.         aux->p0mu[i+2] =  s0[i] * p[0][5]
  418.                               + s0[i+2] *  p[1][5]
  419.                               + acc0[i] *  p[2][5];
  420.  
  421.         // Matrix accumulations for state partials
  422.  
  423.         for (j=0;i<4;i++) {
  424.           pi[j][i]   = p[j][0] * s0[i]
  425.                                + p[j][1] * s0[i+2];
  426.           pi[j][i+2] = p[j][2] * s0[i]
  427.                                + p[j][3] * s0[i+2];
  428.         }
  429.       }
  430.  
  431.       for (i=0;i<3;i++) {
  432.        for (j=0;j<3;j++) {
  433.          p[i][j]   = s[i] * pi[0][j]   + s[i+2] * pi[1][j]
  434.                             + u * s[i+2] * acc0[j];
  435.          p[i][j+3] = s[i] * pi[0][j+2] + s[i+2] * pi[1][j+2]
  436.                           - u * s[i+2] * s0[j+2];
  437.          p[i+2][j] = s[i] * pi[2][j] + s[i+2] * pi[3][j] 
  438.                             + u * acc[i] * acc0[j];
  439.          p[i+2][j+2] = s[i] * pi[2][j+2] + s[i+2] * pi[3][j+2]
  440.                            -u * acc[i] * s0[j+3];
  441.        }
  442.  
  443.        p[i][i]      = p[i][i]     + fm1 + 1;
  444.          p[i][i+2]    = p[i][i+2]   + g;
  445.        p[i+2][i]    = p[i+2][i]   + fd;
  446.        p[i+2][i+2]  = p[i+2][i+2] + gdm1 + 1;
  447.      }
  448.  
  449.      // Transpositions for inverse state partials
  450.      for (i=0;i<3;i++) {
  451.        for (j=0;j<3;j++) {
  452.          pi[j+2][i+2]   =  p[i][j];
  453.          pi[j+2][i]     = -p[i+2][j];
  454.          pi[j][i+2]     = -p[i][j+2];
  455.          pi[j][i]       =  p[i+2][j+2];
  456.        }
  457.      }
  458.  
  459.      // Output state partials and inverse state partials.
  460.      for (i=0;i<6;i++) {
  461.        for (j=0;j<6;j++) {
  462.          aux->p[i][j]  = p[i][j];
  463.          aux->pi[i][j] = pi[i][j];
  464.        }
  465.      }
  466.    } 
  467.  
  468.    return(0);
  469. } 
  470.  
  471.